/*常用串口指令
波特率9600 8 n 1 注意选择端口号
00 01 00 00 00 00  停止 
00 02 00 00 00 00  正转 
00 03 00 00 00 00  反转 
00 05 00 00 00 00  减速 
00 06 00 00 00 00  加速
00 52 00 xx 00 00  串口地址码修改
00 57 xx xx 00 00  速度细控  //2020年04月03号改为00 57 00 nn xx xx  ；nn是速度等级 xxxx是速度值
00 45 00 xx 00 00  速度等级 
00 4a 00 xx 00 00  转轴锁定xx=01锁定 00不锁定02-4 锁定一段时间释放
00 58 xx xx xx xx  直接输入转轴位置
00 59 xx xx xx xx  转轴位置累加
00 5a xx xx xx xx  转轴位置累减
00 5b xx xx xx xx  强制正转步数
00 5c xx xx xx xx  强制反转步数
00 34 63 00 00 00  恢复出厂值代码（从新通电起效）；
注：xx是输入的参数
 */

// 一圈移动3.2cm左右

#include <math.h>;

/*发送的指令*/
unsigned char zhiling[9][6]{{0x00,0x01,0x00,0x00,0x00,0x00}, //停止
                  {0x00,0x02,0x00,0x00,0x00,0x00}, //正转
                  {0x00,0x03,0x00,0x00,0x00,0x00}, //反转
                  {0x00,0x05,0x00,0x00,0x00,0x00}, //减速
                  {0x00,0x06,0x00,0x00,0x00,0x00}, //加速
                  {0x00,0x34,0x63,0x00,0x00,0x00}, //初始化（发送完需要从新通电生效）
                  {0x00,0x59,0x00,0x00,0x01,0x00}, //正转一定步数
                  {0x00,0x5c,0x00,0x00,0x03,0x00},  //反转离开限位开关步数
                  {0x00,0x5c,0x00,0x00,0x19,0x00},  //反转一定步数
                  }; 
// 翻转的初始化指令
unsigned char zhilingInit[6] = {0x00,0x5c,0x00,0x00,0x00,0x00};
// 查询位置的指令
unsigned char zhilingGetPos[6] = {0x00,0x67,0x00,0x00,0x00,0x00};
// 查询体重秤信息指令
unsigned char getWeight[5] = {0xA3,0x00,0xA2,0xA4,0xA5};
                  
float waistPercent = 0.60;
float chestPercent = 0.78;
float shoulderPercent = 0.84;
float hipsPercent = 0.49; 
int personHeight = 162; 
int lock1 = 0; // 驱动未达位置的锁变量 
int lock2 = 0; // 驱动未达位置的锁变量
float base = 50;
long step[4];      

void convert(long step, unsigned char* zhiling) {
  long zhilingBit = step;   // 步数变量
  unsigned char zhilingInit[6] = {0x00,0x5c,0x00,0x00,0x00,0x00};
  for(int i = 0; i < 6; i++) {
    zhiling[i] = zhilingInit[i];
  }
  int index = 5;
  while (zhilingBit > 0) {
    long tempBit = zhilingBit % 256;
    zhiling[index--] = tempBit;
    zhilingBit = zhilingBit / 256;
  }
}

void asyncRunStep (long step) {
  // 要走的步数
  Serial.println(step);
  // 根据步数转化成控制板指令
  convert(step, zhilingInit);
//  delay(100);
  
  // 驱动2至指定步数处
  Serial3.write(zhilingInit,6);
  // 驱动1至指定步数处
  Serial1.write(zhilingInit,6);

  // 开始移动信号
  Serial.println("startMove");
  
  lock1 = 1;
  lock2 = 1;
  // 清空驱动命令返回的串口缓存
  delay(200);
  while(Serial3.available() > 0) {
    Serial3.read();
  }
  while(Serial1.available() > 0) {
    Serial1.read();
  }
  // 等待1，2到达位置
  while (lock1 == 1) {
    delay(200);
    Serial3.write(zhilingGetPos,6);
    int count = 0;
    while(Serial3.available() > 0){
      byte serialData[7];
      Serial3.readBytes(serialData, 7);
      if (serialData[5] == 0) lock1 = 0;
    }
  }
  while (lock2 == 1) {
    delay(200);
    Serial1.write(zhilingGetPos,6);
    int count = 0;
    while(Serial1.available() > 0){
      byte serialData[7];
      Serial1.readBytes(serialData, 7);
      if (serialData[5] == 0) lock2 = 0;
    }
  }
}

void startMeasure (String doneStr, String recvStr, String overStr) {
  // 等待主机的回应
  String recv = "";  
  while (recv != recvStr) {
    Serial.println(doneStr);
    delay(1000);
    recv = "";
    while (Serial.available() > 0) {
      recv += char(Serial.read());
    }
//    Serial.readBytes(serialData, 5);
//    recv = serialData;
    Serial.println(recv);
  }
  Serial.println("ok");
  while (recv != doneStr) {
    delay(1000);
    recv = "";
    while (Serial.available() > 0) {
      recv += char(Serial.read());
    }
  }
  Serial.println(overStr);
}

void BackToStart () {
  Serial1.write(zhiling[1],6);
  Serial3.write(zhiling[1],6);
}

void measureOnce (long *step, bool isNext) {
  // 到臀的位置
  asyncRunStep(step[0]);
  startMeasure("done1", "recv1", "over1");

  // 到腰的位置
  asyncRunStep(step[1]);
  startMeasure("done2", "recv2", "over2");

  // 到胸的位置
  asyncRunStep(step[2]);
  startMeasure("done3", "recv3", "over3");

  if (isNext) {
    // 到肩膀的位置
    asyncRunStep(step[3]);
    startMeasure("done4", "recv4", "over4");
  }

  // 测量完成返回原点
  BackToStart();
}

// 获取被测者的体重信息
int measureWeight () {
  while(Serial2.available() > 0) {
    Serial2.read();
  }
  Serial2.write(getWeight,5);
  delay(20);
  while(Serial2.available() > 0){
    byte serialData[10];
    Serial2.readBytes(serialData, 10);
//    for (int i = 0; i < 10;i++) {
//      Serial.print(serialData[i], HEX);
//    }
//    Serial.println("");
    return int(serialData[6]);
  }
  return -1;
}

void setup() {
    // 初始化串口
  // 串口0接电脑
  Serial.begin(9600);
  // 串口1和串口3接驱动板
  Serial1.begin(9600);
  // 串口2接体重传感器
  Serial2.begin(9600);
  Serial3.begin(9600);
  Serial.print("ok\n");//发送初始化成功
}
void loop(){
  String recv = ""; 
  while (Serial.available() > 0) {
    delay(20);
    recv += char(Serial.read());
  }
  delay(1000);
  if (recv.substring(0, 5) == "begin" || recv.substring(0, 4) == "next") {
    if (recv.substring(0, 5) == "begin") {
      int weight = 0;
      String title = "weight";
      Serial.println("start working");
      weight = measureWeight();
      Serial.println(title + weight);
      
      // 计算移动距离
      personHeight = float(recv.substring(5).toInt());
      float height = personHeight * hipsPercent;
//      这里需要加上体重秤的高度
      float distance = height - base + 5;
      step[0] = (long)(distance / 3.2 * 6400);
      distance = personHeight * (waistPercent - hipsPercent);
      step[1] = (long)(distance / 3.2 * 6400);
      distance = personHeight * (chestPercent - waistPercent);
      step[2] = (long)(distance / 3.2 * 6400);
      distance = personHeight * (shoulderPercent - chestPercent);
      step[3] = (long)(distance / 3.2 * 6400);
      // 开始移动测量
      measureOnce (step, false);
    } else {
      // 开始移动测量
      measureOnce (step, true);
    } 
  }
  if (recv.substring(0, 5) == "reset") {
    BackToStart();
  }
}
